/*
 * image_feature.cc
 *
 *  Created on: Oct 22, 2011
 *      Author: tqlong
 */

#include <fstream>
#include <boost/foreach.hpp>
#include <opencv/cv.h>
#include <opencv/cvaux.h>  // for cv::FAST()
#include "image_feature.h"

void FeatureList::save(const std::string& fileName)
{
  std::ofstream file(fileName.c_str());
  file << this->size() << std::endl;                // first line is the number of features
  BOOST_FOREACH(const Feature& feature, *this)
  {
    const arma::vec& f = feature.f_;
    int n_elem = f.n_elem;
    file << n_elem << " ";                          // each following line start with the size of the feature
    for (int j = 0; j < n_elem; j++)                // followed by all elements of the feature
      file << f(j) << " ";
    file << std::endl;
  }
}

void FeatureList::load(const std::string& fileName)
{
  std::ifstream file(fileName.c_str());
  int n_feature;
  file >> n_feature;
  this->clear();
  for (int i = 0; i < n_feature; i++)
  {
    int len;
    file >> len; // feature size
    arma::vec f(len);
    for (int j = 0; j < len; j++)
      file >> f(j);
    this->push_back(f);
  }
}

void FeatureList::mark(cv::Mat& M, cv::Scalar color)
{
  BOOST_FOREACH(const Feature& feature, *this)
  {
    const arma::vec& f = feature;
    cv::Point2f p(f(0), f(1));
    cv::circle(M, p, 4, color, 1);
  }
}

void FeatureList::FAST_features(const cv::Mat& inputImage, FeatureList& f_list, int threshold)
{
  std::vector<cv::KeyPoint> keypoints;
  cv::Mat grayImage;
  cv::cvtColor(inputImage, grayImage, CV_RGB2GRAY);
  cv::FAST(grayImage, keypoints, threshold);
  f_list.clear();
  BOOST_FOREACH(const cv::KeyPoint& kp, keypoints)
  {
    f_list.push_back(Feature(arma::vec(2)));
    f_list.back().f_(0) = kp.pt.x;
    f_list.back().f_(1) = kp.pt.y;
  }
}
